#include "postprocess/post_processor.h"

PostProcessor::PostProcessor() noexcept : matcher_(nullptr) {}

PostProcessor::~PostProcessor() {}

PostProcessor &PostProcessor::GetInstance() noexcept {
  static PostProcessor instance;
  return instance;
}

bool PostProcessor::Init(PostProcessorSettings const &settings,
                         std::string &err) noexcept {
  std::lock_guard<std::mutex> lock(mutex_);
  if (is_init_) return true;
  settings_ = settings;
  // do init
  is_init_ = true;
  pre_timestamp_ = 0;
  track_initialization_flag_ = false;
  matcher_.reset(new Mathcer());
  return true;
}

bool PostProcessor::Process(PerceptionResult &perceptionResult,
                            std::string &err) noexcept {
  if (!is_init_) {
    err = "post processor module must do init  at first!";
    return false;
  }
  // for first frame ,tracker need to be initialized
  if (!track_initialization_flag_) {
    std::vector<std::int32_t> objects_index;
    std::vector<TrackedObjectPtr> tracked_objects;
    construct_tracked_objects(perceptionResult.dl_boxes, &tracked_objects);
    range(perceptionResult.dl_boxes, objects_index);  // 生成0~(n-1)的序列
    // to do list
    create_new_tracks(
        tracked_objects,
        objects_index);  // 生成track，将lidar_object转至track_object_

    collect_tracked_results(perceptionResult);

    track_initialization_flag_ = true;
    pre_timestamp_ = perceptionResult.timestamp;
    return true;
  } else {
    uint64_t timestamp{perceptionResult.timestamp};
    double time_diff{static_cast<double>(timestamp - pre_timestamp_) * 0.001};
    pre_timestamp_ = timestamp;
    // A. construct objects for tracking
    std::vector<TrackedObjectPtr> tracked_objects;
    construct_tracked_objects(perceptionResult.dl_boxes, &tracked_objects);

    // B. prediction
    std::vector<Eigen::VectorXf> tracks_predict;
    compute_tracks_predict(tracks_predict, time_diff);

    // C. match objects to tracks
    std::vector<std::pair<int, int>> assignments;
    std::vector<int> unassigned_tracks;
    std::vector<int> unassigned_objects;
    std::vector<ObjectTrackPtr> &tracks = object_tracks_.GetTracks();
    if (matcher_ != nullptr) {
      matcher_->Match(&tracked_objects, tracks, tracks_predict, &assignments,
                      &unassigned_tracks,
                      &unassigned_objects);  // match object and track
    }

    // D. Update Tracks
    // D.1 update tracks with associated objects
    update_assigned_tracks(&tracked_objects, assignments, time_diff);

    // D.2 update tracks without associated objects
    update_unassigned_tracks(tracks_predict, unassigned_tracks, time_diff);

    // D.3 delete track
    delete_lost_tracks();
    // D.4 create new tracks for objects without associated tracks
    create_new_tracks(tracked_objects, unassigned_objects);

    // E. collect tracked results
    collect_tracked_results(perceptionResult);

    return true;
  }
}

void PostProcessor::transform_tracker_from_bbox(
    TrackedObjectPtr *tracker, const BoundingBox &bbox) noexcept {
  (*tracker)->center =
      Eigen::Vector3f{bbox.center.x, bbox.center.y, bbox.center.z};
  (*tracker)->size = Eigen::Vector3f{bbox.length, bbox.width, bbox.height};
  (*tracker)->heading = bbox.heading;
  (*tracker)->r = bbox.r;
  (*tracker)->score = bbox.score;
  (*tracker)->label = bbox.label;
  (*tracker)->polygon = bbox.polygon;
  // initialized,to be determined
  (*tracker)->id = 0;
  (*tracker)->velocity = Eigen::Vector3f{bbox.vx, bbox.vy, 0.0f};
  (*tracker)->acceleration = Eigen::Vector3f{bbox.ax, bbox.ay, 0.0f};
}

void PostProcessor::predict(const PerceptionResult &percetion,
                            double time_diff) noexcept {
  tracker_prediction_objects_.Clear();
  u_int32_t tracker_num = percetion.tracker_objects.GetNumberFilled();
  for (auto i = 0; i < tracker_num; i++) {
  }
}

void PostProcessor::construct_tracked_objects(
    const FixedArray<BoundingBox, kMaxBoundingBoxNum> &dl_boxes,
    std::vector<TrackedObjectPtr> *tracked_objects) {
  uint32_t num_dl_boxes = dl_boxes.GetNumberFilled();
  uint32_t num_tracker = 0;
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.Get(i);
    if (bbox.label <= 5) {
      num_tracker++;
    }
  }

  tracked_objects->clear();
  tracked_objects->resize(num_tracker);
  uint32_t index = 0;
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.Get(i);
    if (bbox.label <= 5) {
      TrackedObjectPtr tracker = std::make_shared<TrackedObject>();
      transform_tracker_from_bbox(&tracker, bbox);
      (*tracked_objects)[index] = tracker;
      index++;
    }
  }
}

void PostProcessor::range(
    const FixedArray<BoundingBox, kMaxBoundingBoxNum> &dl_boxes,
    std::vector<std::int32_t> &objects_index) {
  objects_index.clear();
  uint32_t num_dl_boxes = dl_boxes.GetNumberFilled();
  int insert_index = 0;
  for (std::int32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.Get(i);
    if (bbox.label <= 5) {
      objects_index.push_back(insert_index);
      insert_index++;
    }
  }
}

void PostProcessor::collect_tracked_results(
    PerceptionResult &perceptionResult) {
  int collect_consecutive_invisible_maximum_ = 4;
  int collect_age_minimum_ = 5;

  perceptionResult.output_objects.Clear();
  const std::vector<ObjectTrackPtr> &tracks = object_tracks_.GetTracks();
  for (std::uint32_t i{0}; i < tracks.size(); ++i) {
    TrackedObjectPtr result_obj{tracks[i]->current_object_};
    if (tracks[i]->consecutive_invisible_count_ >
        collect_consecutive_invisible_maximum_) {
      continue;
    }
    if (tracks[i]->age_ < collect_age_minimum_) {
      continue;
    }
    // 将数值赋予输出结构体
    auto &obs = perceptionResult.output_objects.GetNext();

    obs.id = tracks[i]->idx_;
    obs.center = result_obj->center;
    obs.size = result_obj->size;
    obs.heading = result_obj->heading;
    obs.velocity = result_obj->velocity;
    obs.acceleration = result_obj->acceleration;

    //    std::cout << "collect_tracked_results object_tracks_ id = " <<
    //    static_cast<int>(obs.id) << " "
    //              << (int)(tracks[i]->idx_) << " velocity = "
    //              << obs.velocity.x() << " " << obs.velocity.y() << " "
    //              << obs.acceleration.x() << " " << obs.acceleration.y() <<
    //              std::endl;

    if (!perceptionResult.output_objects.IsFull()) {
      perceptionResult.output_objects.Add();
    }
  }
}

void PostProcessor::compute_tracks_predict(
    std::vector<Eigen::VectorXf> &tracks_predict, double time_diff) {
  std::int32_t num_track = object_tracks_.size();
  tracks_predict.resize(num_track);
  std::vector<ObjectTrackPtr> &tracks = object_tracks_.GetTracks();
  for (std::int32_t i = 0; i < num_track; ++i) {
    tracks_predict[i] = tracks[i]->Predict(time_diff);
  }
}

void PostProcessor::create_new_tracks(
    std::vector<TrackedObjectPtr> const &new_objects,
    std::vector<std::int32_t> const &unassigned_objects_index) {
  for (int i = 0; i < unassigned_objects_index.size(); i++) {
    std::int32_t obj_id{unassigned_objects_index.at(i)};
    ObjectTrackPtr track(new ObjectTrack(new_objects[obj_id]));
    object_tracks_.AddTrack(track);
  }
}

void PostProcessor::update_assigned_tracks(
    std::vector<TrackedObjectPtr> *new_objects,
    const std::vector<TrackObjectPair> &assignments, double time_diff) {
  std::vector<ObjectTrackPtr> &tracks = object_tracks_.GetTracks();
  for (size_t i = 0; i < assignments.size(); i++) {
    std::int32_t track_id = assignments[i].first;
    std::int32_t obj_id = assignments[i].second;
    tracks[track_id]->UpdateWithObject(&(*new_objects)[obj_id], time_diff);
  }
}

void PostProcessor::update_unassigned_tracks(
    const std::vector<Eigen::VectorXf> &tracks_predict,
    const std::vector<int32_t> &unassigned_tracks, double time_diff) {
  std::vector<ObjectTrackPtr> &tracks = object_tracks_.GetTracks();
  for (size_t i = 0; i < unassigned_tracks.size(); ++i) {
    std::int32_t track_id = unassigned_tracks[i];
    tracks[track_id]->UpdateWithoutObject(tracks_predict[track_id], time_diff);
  }
}

void PostProcessor::delete_lost_tracks() { object_tracks_.RemoveLostTracks(); }
